#include "pushBrake.h"
#include "main.h"

bool CT_lowerTransientEvent = false;
bool CT_lowerTransientWheelieEvent = false;

/***************************/   
/* Counting Task Variables   */
/*Push Detection Variables */
/***************************/
typedef struct {
  unsigned int CT_delay_after_manual_mode;
  float CT_newSpeedSetoint;
  float CT_speedSetpoint;
  bool CT_push_detected;
  unsigned int CT_push_detected_delay;
  int CT_push_start_delay;
  
  signed int CT_cap_val;
  signed int CT_prev_cap_val;
  bool direction_reverse;
  float CT_speed;            // current speed (initial=0)
  float CT_filter_speed;
} CountingStruct_t;


unsigned int CT_delay_after_manual_mode = 0;
float CT_newSpeedSetoint = 0.0f;
float CT_speedSetpoint = 0.0f;

bool CT_push_detected = false;
unsigned int CT_push_detected_delay=0;
int CT_push_start_delay=0;
bool CT_pushInPush=false;

signed int CT_cap_val=0;
signed int CT_prev_cap_val = 0;
bool direction_reverse = false;
float CT_speed = 0.0f;            // current speed (initial=0)
float CT_filter_speed=0;


float filtered_vals[BUFFER_LENGTH];
float filtered_DD[BUFFER_LENGTH];
signed int diff[BUFFER_LENGTH];
signed int max_diff=0;
signed int doublediff[BUFFER_LENGTH];

float temp_accel,temp_dd;
signed int max_diff_buff=0;

signed int new_diff_buff[NEW_DIFF_BUFF_LEN];
unsigned int new_diff_buff_index=0;
unsigned int speed_buff_index=0;

float speed_buff[SPEED_BUFF_LENGTH];
//float temp_speed;

//  float filter_acc=0;
//  float pre_acc=0;
//  float pre_speed=0;
//  float filter_dd=0;
//  float ratio=0;
//  float log_speed=0;

void CT_pushMode_updateSpeedSetpoint(void) // (not for brake) hand released from hand rim, speed starts decreasing b/c push ended
{
  if(CT_push_start_delay>=2)
  {   
    if(CT_pushMode_getNewSetPoint() > CT_speedSetpoint*1.05)
    {     
      if(CT_getPushInPush())
      {
//        buzzer(1);
        if( max_diff>20) // downhill if max_diff < 20 -- max accel for last ten points
        {
          CT_speedSetpoint = CT_pushMode_getNewSetPoint();    
        }
      }
      else  // for first push
      {
        CT_speedSetpoint = CT_pushMode_getNewSetPoint();
      }
    }
  }
  CT_setPushInPush(false);
  CT_push_detected = false;
  CT_resetNewSpeedSetpoint();
}
void CT_pushMode_initPushSequence(void)
{
  CT_push_detected = true;
  CT_push_start_delay=0;
  CT_push_detected_delay=0;
}
void CT_pushMode_initPushInPushSequence(void)
{
  // will start looking for new speed decrease now
  CT_pushMode_initPushSequence();
  CT_setPushInPush(true);
}
void CT_pushMode_disable(void)
{
  CT_setPushDetected(false);
  CT_resetSpeedSetpoint();
  CT_resetDelayAfterManual();
//  CT_pushMode_setNewSpeedSetpoint(0.0f); // inserting this line disables the motor from doing MANUAL mode; the beeps still work; AUTO mode worked
}
void CT_pushMode_checkForFirstPush(void)
{
//    if(CT_push_detected_delay<20)
//    {
//      CT_push_detected_delay++;
//    }
//    if( CT_push_detected_delay>10)//4) // wait 1 second after push or brake, normally value is normally 20
 //   {
      if(CT_pushMode_isFirstPushValid() == true)//4) // wait 1 second after push or brake, normally this is 20
      {              
 //         buzzer_loadDoubleBeep();
          CT_pushMode_initPushSequence();
      }
//    }
 }
void CT_pushMode_respondToFiltAccelTooHigh(void)
{
  CT_pushMode_resetPushInPushDetection();
  CT_push_detected_delay=0;
}
void CT_pushMode_resetPushInPushDetection(void)
{
  CT_push_detected = false;
  CT_resetNewSpeedSetpoint();
  CT_setPushInPush(false);
  //CT_push_detected_delay=0;
}
void CT_pushMode_respondToPush(void)
{
//  max_diff= max_diff > max_diff_buff ? max_diff : max_diff_buff;     
  float current_speed = CT_get_speed();             
  CT_push_start_delay++;
  if(CT_pushMode_pushTooLongLikelyDownhill() == true)        // pushing time is too long, not a push; this means that we detected a push that lasted longer than 1 second, which
  {                              // is interpreted as rolling down a hill and picking up velocity. A push lasts 500 to 600 ms, so we should see a 
                                  // decrease in speed after 5 or 6 CT_push_start_delay ticks.
      CT_pushMode_resetPushInPushDetection();
  }
  else
  {
    if (CT_pushMode_isMaxSpeedReached() == true)      //speed start decrease, max speed reached
    {
       CT_pushMode_updateSpeedSetpoint();      
    }
    else          // new speed is still greater than last, but push still less time than "downhill"
    {
//                             if(filtered_vals[BUFFER_LENGTH -1]<50)    
        if((CT_pushMode_isFiltAccelUnderMaxThresh() == true)&&(CT_getLowerTransient() == false))//&&(UA_getUpperTransient() == false))
//        if((CT_pushMode_isFiltAccelUnderMaxThresh() == true))
        {
          CT_pushMode_updateNewSpeedSetpoint(current_speed);
        }
        else
        { 
            CT_pushMode_respondToFiltAccelTooHigh();        
        }
    }
  }
}

void CT_pushMode_main(void)
{
    if (CT_delay_after_manual_mode < DELAY_ON_ENTRY_TO_AUTO_MODE)   // If we were in Manual Mode recently, but user switches to AUTO_MODE, must wait time period
    {
        CT_delay_after_manual_mode++;
        CT_resetNewSpeedSetpoint();
//        CT_speedSetpoint=0.0f;
    }
    else    // if Button has been released for a certain amount of time
    {
        if(CT_push_detected_delay<20)
        {
          CT_push_detected_delay++;
        }
        if(CT_speedSetpoint<=0.0f)
        {
          if( CT_push_detected_delay>10)//4) // wait 1 second after push or brake, normally value is normally 20
            CT_pushMode_checkForFirstPush();
        }
 //       CT_checkForLowerTransient();
        
        
        //else if ( CT_push_detected )
//        if ((CT_getLowerTransient() == false)&&(UA_getUpperTransient() == false)&&(CT_push_detected))
       if ( CT_push_detected )
        {  
           CT_pushMode_respondToPush();
        }
        else
        {
//          buzzer(0);
 //         max_diff=0;
        }
    }
}
void CT_kinematic_init(void)
{
//    max_diff_buff=0;
    new_diff_buff_index=0;

    for (int i =0; i<BUFFER_LENGTH; i++)
    {
        filtered_vals[i] = 0;
        filtered_DD[i] = 0;
        diff[i] = 0;
        doublediff[i] = 0;
    }
    for (int i =0; i<NEW_DIFF_BUFF_LEN; i++)
    {
       new_diff_buff[i] = 0;
    }
    for (int i =0;i<SPEED_BUFF_LENGTH;i++)
    {
       speed_buff[i]=0;
    }
}
void CT_kinematic_filter_prep(void)
{
    temp_accel = 0.0f;
    temp_dd = 0.0f;
//    max_diff_buff = 0;
    new_diff_buff[new_diff_buff_index % NEW_DIFF_BUFF_LEN] = diff[BUFFER_LENGTH-1];
    new_diff_buff_index++;
    max_diff=0;

    for (int i =0; i<NEW_DIFF_BUFF_LEN; i++)
    {
        max_diff= (max_diff > new_diff_buff[i]) ? max_diff : new_diff_buff[i];
    }
    for (int i=0;i<(BUFFER_LENGTH-1);i++)
    {
        diff[i] = diff[i+1];
  //      max_diff_buff= (max_diff_buff > diff[i]) ? max_diff_buff : diff[i];
        doublediff[i] = doublediff[i+1];
        filtered_vals[i] = filtered_vals[i+1];
        filtered_DD[i] = filtered_DD[i+1];
        temp_accel += diff[i];
        temp_dd += doublediff[i];
    }
}

void CT_kinematic_filter_update(void)
{
    diff[BUFFER_LENGTH -1]       = CT_get_cap_val() - CT_get_prev_cap_val();
    doublediff[BUFFER_LENGTH -1] = diff[BUFFER_LENGTH-1] - diff[BUFFER_LENGTH-2];
    filtered_vals[BUFFER_LENGTH-1] = ((float)diff[BUFFER_LENGTH-1]       + temp_accel) / ((float)BUFFER_LENGTH);
    filtered_DD[BUFFER_LENGTH-1]   = ((float)doublediff[BUFFER_LENGTH-1] + temp_dd   ) / ((float)BUFFER_LENGTH);
}
float CT_butterworth_filter_update(unsigned int x_val)
{
        //butter filter
    static float filter_b[5]={0.0004f,0.0017f,0.0025f,0.0017f,0.0004f};
    static float filter_a[5]={1.0000f,-3.1806f,3.8612f,-2.1122f,0.4383f};
    static float filter_z[5]={0,0,0,0,0};
  
  //  float filter_x=0;
    static float filter_y=0;
    //  
 //   sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
    filter_y    = filter_b[0]*x_val + filter_z[0];
    filter_z[0] = filter_b[1]*x_val + filter_z[1] - filter_a[1]*filter_y;
    filter_z[1] = filter_b[2]*x_val + filter_z[2] - filter_a[2]*filter_y;
    filter_z[2] = filter_b[3]*x_val + filter_z[3] - filter_a[3]*filter_y;
    filter_z[3] = filter_b[4]*x_val               - filter_a[4]*filter_y;
    
    return filter_y;
}
float CT_average_filter_update(unsigned int x_val)
{
    float filter_y=0;
    float temp_speed = 0;
    //  
 //   sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
    speed_buff[speed_buff_index % SPEED_BUFF_LENGTH]= (float)x_val;
    speed_buff_index++;
//    temp_speed=0;
    for (int i =0;i<SPEED_BUFF_LENGTH;i++)
    {
       temp_speed+=speed_buff[i];
    }
    filter_y = temp_speed/((float)SPEED_BUFF_LENGTH);
    
    return filter_y;
}
void CT_get_encoder_SpeedAndDirection(void)
{
  CT_set_prev_cap_val(CT_get_cap_val());

  portENTER_CRITICAL();
  {
      CT_set_cap_val(T1TC);
      T1TCR = (1<<1);                      // reset and disable Timer0
      T1TCR = 1;                           // enable Timer0
      if(P0PIN&(1<<29))                 //backward
      {
        CT_set_cap_val(0);
        set_direction_reverse(true);
      }
      else {
        set_direction_reverse(false);
      }
  }
  portEXIT_CRITICAL();
}
void CT_pushMode_updateNewSpeedSetpoint(float curr_speed)
{
    if ( curr_speed < 1.0f )
        curr_speed = 0;
    CT_setNewSpeedSetpoint(curr_speed);
}
bool CT_pushMode_isFirstPushValid(void)
{
//  if( CT_push_detected_delay>10)//4) // wait 1 second after push or brake, normally value is normally 20
//  {              
    if(CT_pushMode_isAFirstPush() == true)           
    {               
        return true;
    }
//   }
   return false;
}


/// former inlines
/*inline*/ void CT_setPushDetected(bool val)
{
  CT_push_detected = val;
}
/*inline*/ bool CT_getPushDetected(void)
{
  return CT_push_detected;
}
/*inline*/ void CT_resetDelayAfterManual(void)
{
  CT_delay_after_manual_mode = 0;
}
/*inline*/ void CT_resetPushStartDelay()
{
  CT_push_start_delay = 0;
}
/*inline*/ void CT_resetPushDetectedDelay()
{
  CT_push_detected_delay = 0;
}
/*inline*/ unsigned int CT_getPushDetectedDelay(void)
{
  return CT_push_detected_delay;
}
/*inline*/ void CT_set_speed (uint32_t freq, float _spd)
{
//  sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
  CT_speed = ( (float)_spd  * ((float)freq) * (WHEEL_RADIUS) ) / ( TICKS_PER_REVOLUTION*((float)2.801127) ); 
}
/*inline*/ float CT_get_speed (void)
{
  return CT_speed;
}
/*inline*/ void CT_set_filter_speed (uint32_t freq, float _spd)
{
//  sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
  CT_filter_speed = ( (float)_spd  * ((float)freq) * (WHEEL_RADIUS) ) / ( TICKS_PER_REVOLUTION*((float)2.801127) ); 
}
/*inline*/ float CT_get_filter_speed (void)
{
  return CT_filter_speed;
}
/*inline*/ void CT_set_cap_val (signed int _cap_val)
{
  CT_cap_val = _cap_val; 
}
/*inline*/ signed int CT_get_cap_val (void)
{
  return CT_cap_val;
}
/*inline*/ void CT_set_prev_cap_val (unsigned int _cap_val)
{
  CT_prev_cap_val = _cap_val; 
}
/*inline*/ unsigned int CT_get_prev_cap_val (void)
{
  return CT_prev_cap_val;
}
/*inline*/ void set_direction_reverse (bool _dir)
{
  direction_reverse = _dir; 
}
/*inline*/ bool get_direction_reverse (void)
{
  return direction_reverse;
}
/*inline*/ bool CT_pushMode_isFiltAccelUnderMaxThresh(void)
{
  return (filtered_vals[BUFFER_LENGTH -1] < 300);
}
/*inline*/ bool CT_pushMode_isFiltAccelOverMinThresh(void)
{
  return (filtered_vals[BUFFER_LENGTH -1] > 16);
}
/*inline*/ bool CT_pushMode_isMaxAccelUnderThresh(void)
{
  return (CT_getLastAccel()<300);
}
/*inline*/ bool CT_getPushInPush(void)
{
  return CT_pushInPush;
}
/*inline*/ void CT_setPushInPush(bool val)
{
  CT_pushInPush = val;
}
/*inline*/ bool CT_pushMode_isAccelIncreasing(void)
{
  return (filtered_DD[BUFFER_LENGTH -1] > 0);
}
/*inline*/ bool CT_pushMode_isAFirstPush(void)
{
  return ((CT_pushMode_isFiltAccelOverMinThresh() == true) && (CT_pushMode_isFiltAccelUnderMaxThresh() == true) && (CT_pushMode_isAccelIncreasing() == true));
}
/*inline*/ void CT_setNewSpeedSetpoint(float setPoint)
{
    CT_newSpeedSetoint = setPoint;
}
/*inline*/ void CT_resetNewSpeedSetpoint(void)
{
    CT_newSpeedSetoint = 0.0f;
}
/*inline*/ void CT_resetSpeedSetpoint(void)
{
  CT_speedSetpoint = 0.0f;
}
/*inline*/ void CT_setSpeedSetpoint(float val)
{
  CT_speedSetpoint = val;
}
/*inline*/ float CT_getSpeedSetpoint(void)
{
  return CT_speedSetpoint;
}
/*inline*/ float CT_pushMode_getNewSetPoint(void)
{
  return CT_newSpeedSetoint;
}
/*inline*/ bool CT_pushMode_pushTooLongLikelyDownhill(void)
{
  // pushing time is too long, not a push; this means that we detected a push that lasted longer than 1 second, which
  // is interpreted as rolling down a hill and picking up velocity. A push lasts 500 to 600 ms, so we should see a 
  // decrease in speed after 5 or 6 CT_push_start_delay ticks.
  return (CT_push_start_delay>10);
}
/*inline*/ bool CT_pushMode_isMaxSpeedReached(void)
{
  return (CT_get_speed() < CT_pushMode_getNewSetPoint());
}
/*inline*/ void CT_setLowerTransient(bool val)
{
  CT_lowerTransientEvent = val;
}
/*inline*/ bool CT_getLowerTransient(void)
{
  return (CT_lowerTransientEvent);//>>4);
}
/*inline*/ void CT_setLowerTransientWheelie(bool val)
{
  CT_lowerTransientWheelieEvent = val;
}
/*inline*/ bool CT_getLowerTransientWheelie(void)
{
  return (CT_lowerTransientWheelieEvent);//>>4);
}
/*inline*/ signed int CT_getLastAccel(void)
{
  return diff[BUFFER_LENGTH-1];
}
